clc;clear;close all;
I = readFigure('.\706459\Pic_');
mask_filepath = '.\0.4+0.3.bmp';
Area = [2530, 2830,3550, 3850];
T = 20;
[I, MASK] = fun_mask(mask_filepath, I, Area, T);
MASK(isnan(MASK)) = 0;
[M,N] = size(MASK);
warpPhase = zeros(M,N,3);
lambda = [1/70 1/64 1/59];
lambda12 = lambda(2) * lambda(1) / (lambda(2) - lambda(1));
lambda23 = lambda(2) * lambda(3) / (lambda(3) - lambda(2));
lambda123 = lambda23 * lambda12 / (lambda23 - lambda12);
for i = 1:3
    index = (i-1)*4+12;
    warpPhase(:,:,i) = atan2(I{4+index,1} - I{2+index,1}, I{1+index,1} - I{3+index,1});  
end

PHI_C1 = warpPhase(:,:,1);
PHI_C2 = warpPhase(:,:,2);
save('stdPhi.mat','PHI_C2');
PHI_C3 = warpPhase(:,:,3);
% unWarpPhase = GoldsteinUnwrap2D_r1(PHI_C1, MASK);
PHI_C12 = PHI_C1 - PHI_C2;
PHI_C12(PHI_C12<0) = PHI_C12(PHI_C12<0) + 2*pi;

PHI_C23 = PHI_C2 - PHI_C3;
PHI_C23(PHI_C23<0) = PHI_C23(PHI_C23<0) + 2*pi;

PHI_C123 = PHI_C12 - PHI_C23;
PHI_C123(PHI_C123<0) = PHI_C123(PHI_C123<0) + 2*pi;

phi_c23 = PHI_C23+2*pi*round((PHI_C123*lambda123/lambda23-PHI_C23)/(2*pi));
phi_c12 = PHI_C12+2*pi*round((PHI_C123*lambda123/lambda12-PHI_C12)/(2*pi));

phi_c1 = PHI_C1+2*pi*round((phi_c12*lambda12/lambda(1)-PHI_C1)/(2*pi));
phi_c2 = PHI_C2+2*pi*round((phi_c12*lambda12/lambda(2)-PHI_C2)/(2*pi));
phi_c3 = PHI_C3+2*pi*round((phi_c23*lambda23/lambda(3)-PHI_C3)/(2*pi));

phi_c2 = imgaussfilt(phi_c2,[3,3]);
save('64-unwarpphase.mat','phi_c2');
unWarpPhase = nan(3648,5472);
unWarpPhase(Area(1):Area(2),Area(3):Area(4)) = phi_c2;

% unWarpPhase1(Area(1):Area(2),Area(3):Area(4)) = unWarpPhase;
ptCloud = Reconstruct(unWarpPhase);
pcwrite(ptCloud,'.\706459\64.ply');
fprintf('点云生成！\n');